import numpy as np


def objective_function(x, y):
    return (x + 1) ** 3 / 3 + y

def penalty_term(x, y, u):
    # 对于 x-1 >= 0，我们使用 1/(x-1+1/u) 来惩罚 x < 1 的情况
    # 对于 y >= 0，我们使用 1/y 来惩罚 y < 0 的情况（但注意这会在 y=0 时导致无穷大，所以我们需要一个小的正数来避免这种情况）
    return u * (1 / (np.maximum(x - 1, 1e-6) + 1 / u) + 1 / (np.maximum(y, 1e-6)))


def penalized_objective(x, y, u):
    return objective_function(x, y) + penalty_term(x, y, u)


def gradient_penalized_objective(x, y, u):
    # 计算梯度（这里需要手动计算）
    grad_x = (x + 1) ** 2 - u / ((x - 1 + 1 / u) ** 2)
    grad_y = 1 - u / (y + 1 / u) ** 2
    return np.array([grad_x, grad_y])


# 初始化参数
x0 = np.array([3, 4])
u = 10
c = 0.1
epsilon = 1e-5
learning_rate = 0.1

# 迭代过程
while True:
    # 计算当前惩罚后的目标函数值和梯度
    f_penalized = penalized_objective(x0[0], x0[1], u)
    grad = gradient_penalized_objective(x0[0], x0[1], u)
    # 输出当前迭代信息
    print(f"x = {x0[0]}, y = {x0[1]}, u = {u}, penalized objective = {f_penalized}")
    # 更新x和y（使用梯度下降）
    x0 =x0 - learning_rate * grad
    # 检查收敛性
    if np.linalg.norm(grad) < epsilon:
        break
        # 如果解接近约束边界，则减小惩罚因子（注意：这里我们简单地根据迭代次数来减小u，而不是根据解与约束的距离）
    # 在实际应用中，你可能需要更复杂的逻辑来确定何时减小u
    if np.all(x0 >= 1) and np.all(x0[1] >= 0):  # 检查是否满足所有约束（注意：这里我们假设y是x0的第二个元素）
        u *= c
    # 输出最终结果
print(f"Final solution: x = {x0[0]}, y = {x0[1]}, with penalty factor u = {u}")